/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "MachineCtrlClient.hpp"


using namespace MaCI::MachineCtrl;
//******************************************************************************
CMachineCtrlClient::CMachineCtrlClient(gimi::GIMI *aGIMIPtr,
                                       std::string aMaCIGroup)
  : iGIMI(aGIMIPtr),
    iOwnGIMI(false),
    iMCC(NULL),
    iMaCIGroup(aMaCIGroup),
    iMaCIClients()
{
  iMCC = new MaCI::MaCICtrl::CMaCICtrlClient(iGIMI,-1);
  ConstructClientMaps();
  Update();

}
//******************************************************************************

CMachineCtrlClient::CMachineCtrlClient(std::string aGIMnetAPAddress,
                                       unsigned int aGIMnetAPPort,
                                       std::string aGIMnetName,
                                       std::string aMaCIGroup):
  iGIMI(NULL),
  iOwnGIMI(true),
  iMCC(NULL),
  iMaCIGroup(aMaCIGroup),
  iMaCIClients()
{
  iGIMI = new gimi::GIMI();
  int r;
  if ( (r = iGIMI->connectToHubEx(aGIMnetAPAddress, aGIMnetAPPort, aGIMnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   aGIMnetAPAddress.c_str(), 
	   aGIMnetAPPort, 
           aGIMnetName.size()? aGIMnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
           aGIMnetAPAddress.c_str(), 
	   aGIMnetAPPort, 
           aGIMnetName.size()? aGIMnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
  }
  iMCC = new MaCI::MaCICtrl::CMaCICtrlClient(iGIMI,-1);

  ConstructClientMaps();
  Update();
    
  
}
//******************************************************************************

CMachineCtrlClient::~CMachineCtrlClient(){

  DestructAllClients();

  delete(iMCC);
  if(iOwnGIMI){
    //dPrint(1,"**************deleting GIMI*******");
    delete(iGIMI);
    // dPrint(1,"*********************GIMI deleted**********************************");
    iOwnGIMI = false;
  }

}
//******************************************************************************

void CMachineCtrlClient::ConstructClientMaps(){
  iMaCIClients[KTypeAlarm] = TMaCIBaseClientMap();
  iMaCIClients[KTypeBehaviour] = TMaCIBaseClientMap();
  iMaCIClients[KTypeCoordinateDrive] = TMaCIBaseClientMap();
  iMaCIClients[KTypeEmergencyStop] = TMaCIBaseClientMap();
  iMaCIClients[KTypeEnergy] = TMaCIBaseClientMap();
  iMaCIClients[KTypeImage] = TMaCIBaseClientMap();
  iMaCIClients[KTypeIMU] = TMaCIBaseClientMap();
  iMaCIClients[KTypeIO] = TMaCIBaseClientMap();
  iMaCIClients[KTypeJointGroupCtrl] = TMaCIBaseClientMap();
  iMaCIClients[KTypeMap] = TMaCIBaseClientMap();
  iMaCIClients[KTypeParameter] = TMaCIBaseClientMap();
  iMaCIClients[KTypePosition] = TMaCIBaseClientMap();
  iMaCIClients[KTypeRanging] = TMaCIBaseClientMap();
  iMaCIClients[KTypeSpeedCtrl] = TMaCIBaseClientMap();
  iMaCIClients[KTypeText] = TMaCIBaseClientMap();
  iMaCIClients[KTypeUnit] = TMaCIBaseClientMap();
  iMaCIClients[KTypeWireless] = TMaCIBaseClientMap();

}
//******************************************************************************

bool CMachineCtrlClient::OpenClients(const EInterfaceType &aType)
{
  bool result = true;
  if(aType == KTypeAll){
    TMaCIClientsMap::iterator mcIterator = iMaCIClients.begin();
    while(mcIterator != iMaCIClients.end()){
      TMaCIBaseClientMap::iterator iter = mcIterator->second.begin();

      while(iter != mcIterator->second.end()){
        if(iter->second->Open() !=KMaCIOK){
          result = false;
        }
        iter++;
      }
      mcIterator++;

    }

  }else{
    TMaCIClientsMap::iterator mcIterator = iMaCIClients.find(aType);
    if(mcIterator != iMaCIClients.end()){
      TMaCIBaseClientMap::iterator iter = mcIterator->second.begin();

      while(iter != mcIterator->second.end()){
        if(iter->second->Open() !=KMaCIOK){
          result = false;
        }
        iter++;
      }
    }
  }
 
  return result;
}
//******************************************************************************

bool CMachineCtrlClient::CloseClients(const EInterfaceType &aType)
{

  bool result = true;
  if(aType == KTypeAll){
    TMaCIClientsMap::iterator mcIterator = iMaCIClients.begin();
    while(mcIterator != iMaCIClients.end()){
      TMaCIBaseClientMap::iterator iter = mcIterator->second.begin();
      dPrint(6,"Closing all %d type clients",mcIterator->first);
      while(iter != mcIterator->second.end()){
        if(iter->second->Close() !=KMaCIOK){
          result = false;
        }
        iter++;
      }
      mcIterator++;

    }

  }else{
    TMaCIClientsMap::iterator mcIterator = iMaCIClients.find(aType);
    if(mcIterator != iMaCIClients.end()){
      TMaCIBaseClientMap::iterator iter = mcIterator->second.begin();

      while(iter != mcIterator->second.end()){
        if(iter->second->Close() !=KMaCIOK){
          result = false;
        }
        iter++;
      }
    }
  }
 
  return result;



}
//******************************************************************************

void CMachineCtrlClient::GetMachineComponents(TMaCIMachineComponents &aComponents)
{

  aComponents.clear();
  
  for(EACH_IN_i(iMaCIClients)){
    TMaCIBaseClientMap::iterator iter = i->second.begin();
    while(iter != i->second.end()){
      aComponents.push_back(std::make_pair(i->first,iter->first));
      ++iter;
    }
  }


}
//******************************************************************************

MaCI::CMaCI *CMachineCtrlClient::GetMaCIPointer(EInterfaceType aType, std::string aInstanceName)
{
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(aType);
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator mbcIter = mcmIter->second.find(aInstanceName);
    if(mbcIter != mcmIter->second.end()){
      return mbcIter->second;
      
    }else{
      return NULL;
    }
    
  }else{
    return NULL;
  }

   

}
//******************************************************************************

TAlarmClientMap CMachineCtrlClient::GetAlarmClients(bool aOpen)
{
  
  TAlarmClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeAlarm);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Alarm::CAlarmClient *>(iter->second);
      }
      ++iter;
     
    }

  }
  return map;

}
//******************************************************************************

MaCI::Alarm::CAlarmClient *CMachineCtrlClient::GetAlarmClient(std::string aInstanceName,bool aOpen)
{

  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeAlarm,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
        return NULL;
    }else{
      return dynamic_cast<MaCI::Alarm::CAlarmClient *>(maciptr);
    }
  }else{
    return NULL;
  }

}
//******************************************************************************

TBehaviourClientMap CMachineCtrlClient::GetBehaviourClients(bool aOpen)
{
  
  TBehaviourClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeBehaviour);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Behaviour::CBehaviourClient *>(iter->second);
      }

      ++iter;


    }

  }
  return map;

}
//******************************************************************************

MaCI::Behaviour::CBehaviourClient *CMachineCtrlClient::GetBehaviourClient(std::string aInstanceName,bool aOpen)
{

  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeBehaviour,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
        return NULL;
    }else{
      return dynamic_cast<MaCI::Behaviour::CBehaviourClient *>(maciptr);
    }
    
  }else{
    return NULL;
  }

}
//******************************************************************************
  
TCoordinateDriveClientMap CMachineCtrlClient::GetCoordinateDriveClients(bool aOpen)
{
  TCoordinateDriveClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeCoordinateDrive);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::CoordinateDrive::CCoordinateDriveClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;


}
//******************************************************************************

MaCI::CoordinateDrive::CCoordinateDriveClient *CMachineCtrlClient::GetCoordinateDriveClient(std::string aInstanceName,bool aOpen)
{
  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeCoordinateDrive,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
        return NULL;
    }else{
      return dynamic_cast<MaCI::CoordinateDrive::CCoordinateDriveClient *>(maciptr);
    }
   
  }else{
    return NULL;
  }


}
//******************************************************************************

TEmergencyStopClientMap CMachineCtrlClient::GetEmergencyStopClients(bool aOpen)
{
  TEmergencyStopClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeEmergencyStop);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::EmergencyStop::CEmergencyStopClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;
  

}
//******************************************************************************

MaCI::EmergencyStop::CEmergencyStopClient *CMachineCtrlClient::GetEmergencyStopClient(std::string aInstanceName,bool aOpen)
{
  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeEmergencyStop,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
        return NULL;
    }else{
      return dynamic_cast<MaCI::EmergencyStop::CEmergencyStopClient *>(maciptr);
    }
  
  }else{
    return NULL;
  }

}
//******************************************************************************

TEnergyClientMap CMachineCtrlClient::GetEnergyClients(bool aOpen)
{

  TEnergyClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeEnergy);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end() ){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Energy::CEnergyClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;

}
//******************************************************************************

MaCI::Energy::CEnergyClient *CMachineCtrlClient::GetEnergyClient(std::string aInstanceName,bool aOpen)
{
  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeEnergy,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
        return NULL;
    }else{
      return dynamic_cast<MaCI::Energy::CEnergyClient *>(maciptr);
    }
   
  }else{
    return NULL;
  }

}
//******************************************************************************

TImageClientMap CMachineCtrlClient::GetImageClients(bool aOpen)
{
  TImageClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeImage);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Image::CImageClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;


}
//******************************************************************************

MaCI::Image::CImageClient *CMachineCtrlClient::GetImageClient(std::string aInstanceName,bool aOpen)
{

  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeImage,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::Image::CImageClient *>(maciptr);
    }
  }else{
    return NULL;
  }



}
//******************************************************************************

TIMUClientMap CMachineCtrlClient::GetIMUClients(bool aOpen)
{

  TIMUClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeIMU);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::IMU::CIMUClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;


}
//******************************************************************************

MaCI::IMU::CIMUClient *CMachineCtrlClient::GetIMUClient(std::string aInstanceName,bool aOpen)
{
  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeIMU,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::IMU::CIMUClient *>(maciptr);
    }
  }else{
    return NULL;

  }
}
//******************************************************************************

TIOClientMap CMachineCtrlClient::GetIOClients(bool aOpen)
{

  TIOClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeIO);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::IO::CIOClient *>(iter->second);
      }
      ++iter;
    }

  }
  return map;


}
//******************************************************************************

MaCI::IO::CIOClient *CMachineCtrlClient::GetIOClient(std::string aInstanceName,bool aOpen)
{

  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeIO,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::IO::CIOClient *>(maciptr);
    }
  }else{
    return NULL;

  }
}
//******************************************************************************

TJointGroupCtrlClientMap CMachineCtrlClient::GetJointGroupCtrlClients(bool aOpen)
{

  TJointGroupCtrlClientMap map;
  TMaCIClientsMap::iterator mcmIter = iMaCIClients.find(KTypeJointGroupCtrl);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::JointGroupCtrl::CJointGroupCtrlClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;

}
//******************************************************************************

MaCI::JointGroupCtrl::CJointGroupCtrlClient *CMachineCtrlClient::GetJointGroupCtrlClient(std::string aInstanceName,bool aOpen)
{

MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeJointGroupCtrl,aInstanceName);

 if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::JointGroupCtrl::CJointGroupCtrlClient *>(maciptr);
    }
  }else{
    return NULL;
  }

}
//******************************************************************************

TMapClientMap CMachineCtrlClient::GetMapClients(bool aOpen)
{

  TMapClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeMap);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Map::CMapClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;


}
//******************************************************************************

MaCI::Map::CMapClient *CMachineCtrlClient::GetMapClient(std::string aInstanceName,bool aOpen)
{

  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeMap,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::Map::CMapClient *>(maciptr);
    }
  }else{
    return NULL;
  }

}
//******************************************************************************

TParameterClientMap CMachineCtrlClient::GetParameterClients(bool aOpen)
{

  TParameterClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeParameter);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Parameter::CParameterClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;
  

}
//******************************************************************************
MaCI::Parameter::CParameterClient *CMachineCtrlClient::GetParameterClient(std::string aInstanceName,bool aOpen)
{
  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeParameter,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::Parameter::CParameterClient *>(maciptr);
    }
  }else{
    return NULL;
  }
}

//******************************************************************************

TPositionClientMap CMachineCtrlClient::GetPositionClients(bool aOpen)
{

TPositionClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypePosition);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Position::CPositionClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;
  
}
//******************************************************************************
MaCI::Position::CPositionClient *CMachineCtrlClient::GetPositionClient(std::string aInstanceName,bool aOpen)
{

  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypePosition,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::Position::CPositionClient *>(maciptr);
    }
  }else{
    return NULL;


  }

}
//******************************************************************************

TRangingClientMap CMachineCtrlClient::GetRangingClients(bool aOpen)
{

  TRangingClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeRanging);

  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();

    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Ranging::CRangingClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;
  

}
//******************************************************************************
MaCI::Ranging::CRangingClient *CMachineCtrlClient::GetRangingClient(std::string aInstanceName,bool aOpen)
{

  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeRanging,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::Ranging::CRangingClient *>(maciptr);
    }
  }else{
    return NULL;
  }




}
//******************************************************************************

TSpeedCtrlClientMap CMachineCtrlClient::GetSpeedCtrlClients(bool aOpen)
{

  TSpeedCtrlClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeSpeedCtrl);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::SpeedCtrl::CSpeedCtrlClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;


}
//******************************************************************************
MaCI::SpeedCtrl::CSpeedCtrlClient *CMachineCtrlClient::GetSpeedCtrlClient(std::string aInstanceName,bool aOpen)
{

  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeSpeedCtrl,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::SpeedCtrl::CSpeedCtrlClient *>(maciptr);
    }
  }else{
    return NULL;

  }
  
}
//******************************************************************************

TTextClientMap CMachineCtrlClient::GetTextClients(bool aOpen)
{


  TTextClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeText);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Text::CTextClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;
  
}
//******************************************************************************

MaCI::Text::CTextClient *CMachineCtrlClient::GetTextClient(std::string aInstanceName,bool aOpen)
{
  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeText,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::Text::CTextClient *>(maciptr);
    }
  }else{
    return NULL;

  }


}
//******************************************************************************

TUnitClientMap CMachineCtrlClient::GetUnitClients(bool aOpen)
{
TUnitClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeUnit);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Unit::CUnitClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;
  
}
//******************************************************************************

MaCI::Unit::CUnitClient *CMachineCtrlClient::GetUnitClient(std::string aInstanceName,bool aOpen)
{
  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeUnit,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::Unit::CUnitClient *>(maciptr);
    }
  }else{
    return NULL;
  }


}
//******************************************************************************

TWirelessClientMap  CMachineCtrlClient::GetWirelessClients(bool aOpen)
{
TWirelessClientMap map;
  TMaCIClientsMap::iterator mcmIter=  iMaCIClients.find(KTypeWireless);
  
  if(mcmIter != iMaCIClients.end()){
    TMaCIBaseClientMap::iterator iter = mcmIter->second.begin();
    while(iter != mcmIter->second.end()){
      if(!aOpen || (aOpen &&  iter->second->Open() != MaCI::KMaCIOK)){
        map[iter->first] =  dynamic_cast<MaCI::Wireless::CWirelessClient *>(iter->second);
      }
      ++iter;


    }

  }
  return map;
 
}
//******************************************************************************

MaCI::Wireless::CWirelessClient *CMachineCtrlClient::GetWirelessClient(std::string aInstanceName,bool aOpen)
{
  MaCI::CMaCI *maciptr= GetMaCIPointer(KTypeWireless,aInstanceName);

  if(maciptr != NULL){
    if(aOpen && maciptr->Open() != MaCI::KMaCIOK){
      return NULL;
    }else{
      return dynamic_cast<MaCI::Wireless::CWirelessClient *>(maciptr);
    }
  }else{
    return NULL;

  }


}
//******************************************************************************

void CMachineCtrlClient::Update()
{
  gimi::t_serviceList gimiServiceList;
 

  gimi::t_serviceList servicelist;
  iMCC->DiscoverMaCICtrlServices(servicelist, 3000);
 
      
 
  MaCI::MaCICtrl::TServiceEntryArray entries;
  MaCI::MaCICtrl::TServiceEntryArray entriesFiltered;
  MaCI::MaCICtrl::SMaCISL maciSlFilter(iMaCIGroup,"","");
  if (iMCC->GetServiceArray(entries, servicelist) == MaCI::KMaCIOK) {
   
   
    if(iMCC->FilterServices(entriesFiltered,
                           entries,
                            maciSlFilter) == MaCI::KMaCIOK){




   
      
      std::string lastsl;
      for(EACH_IN_i(entriesFiltered)) {
        /* const std::string currentsl = i->GetMaCISL().ToMaCISLString();
        
        if (currentsl != lastsl) {
          dPrint(9,"'%s' @ MaCISL:\t'%s'", 
                 i->gimnetname.c_str(),
                 currentsl.c_str());
                 lastsl = currentsl;*/
         
          //        }
        ConstructMaCIClient(i->GetMaCISL(),*i);
      }
    }else{
      dPrint(3,"Failed to filter services when updating machine information");
    }
  } else {
    dPrint(3,"Failed to get service array from the network!");
        
  }
}
//******************************************************************************

void CMachineCtrlClient::ConstructMaCIClient(MaCI::MaCICtrl::SMaCISL aMaCISl,MaCI::MaCICtrl::SServiceEntry aService){
  using namespace MaCI;
  dPrint(3,"Constructing MaCI Client for service '%s'",aMaCISl.ToMaCISLString().c_str());
  if(aMaCISl.interface == "MaCI_Alarm"){
  
    if(iMaCIClients[KTypeAlarm].find(aMaCISl.instance) == iMaCIClients[KTypeAlarm].end()){
     
      Alarm::CAlarmClient *c = new Alarm::CAlarmClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Alarm Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeAlarm].insert(make_pair(aMaCISl.instance,c));
    }

    
  }else if(aMaCISl.interface == "MaCI_Behaviour"){
  
    if(iMaCIClients[KTypeBehaviour].find(aMaCISl.instance) == iMaCIClients[KTypeBehaviour].end()){
     
      Behaviour::CBehaviourClient *c = new Behaviour::CBehaviourClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Behaviour Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeBehaviour].insert(make_pair(aMaCISl.instance,c));
    }

    
  }else if(aMaCISl.interface == "MaCI_CoordinateDrive"){
    if(iMaCIClients[KTypeCoordinateDrive].find(aMaCISl.instance) == iMaCIClients[KTypeCoordinateDrive].end()){

      CoordinateDrive::CCoordinateDriveClient *c = new CoordinateDrive::CCoordinateDriveClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a CoordinateDrive Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeCoordinateDrive].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_EmergencyStop"){
    if(iMaCIClients[KTypeEmergencyStop].find(aMaCISl.instance) == iMaCIClients[KTypeEmergencyStop].end()){

      EmergencyStop::CEmergencyStopClient *c = new EmergencyStop::CEmergencyStopClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a EmergencyStop Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeEmergencyStop].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_Energy"){
    if(iMaCIClients[KTypeEnergy].find(aMaCISl.instance) == iMaCIClients[KTypeEnergy].end()){

      Energy::CEnergyClient *c = new Energy::CEnergyClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Energy Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeEnergy].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_Image"){
    if(iMaCIClients[KTypeImage].find(aMaCISl.instance) == iMaCIClients[KTypeImage].end()){

      Image::CImageClient *c = new Image::CImageClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Image Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeImage].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_IMU"){
    if(iMaCIClients[KTypeIMU].find(aMaCISl.instance) == iMaCIClients[KTypeIMU].end()){

      IMU::CIMUClient *c = new IMU::CIMUClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a IMU Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeIMU].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_IO"){
    if(iMaCIClients[KTypeIO].find(aMaCISl.instance) == iMaCIClients[KTypeIO].end()){

      IO::CIOClient *c = new IO::CIOClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a IO Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeIO].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_JointGroupCtrl"){
    if(iMaCIClients[KTypeJointGroupCtrl].find(aMaCISl.instance) == iMaCIClients[KTypeJointGroupCtrl].end()){

      JointGroupCtrl::CJointGroupCtrlClient *c = new JointGroupCtrl::CJointGroupCtrlClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a JointGroupCtrl Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeJointGroupCtrl].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_Map"){
    if(iMaCIClients[KTypeMap].find(aMaCISl.instance) == iMaCIClients[KTypeMap].end()){

      Map::CMapClient *c = new Map::CMapClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Map Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeMap].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_Parameter"){
    if(iMaCIClients[KTypeParameter].find(aMaCISl.instance) == iMaCIClients[KTypeParameter].end()){

      Parameter::CParameterClient *c = new Parameter::CParameterClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Parameter Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeParameter].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_Position"){
    if(iMaCIClients[KTypePosition].find(aMaCISl.instance) == iMaCIClients[KTypePosition].end()){

      Position::CPositionClient *c = new Position::CPositionClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Position Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypePosition].insert(make_pair(aMaCISl.instance,c));
    }

  }else if(aMaCISl.interface == "MaCI_Ranging"){
    if(iMaCIClients[KTypeRanging].find(aMaCISl.instance) == iMaCIClients[KTypeRanging].end()){

      Ranging::CRangingClient *c = new Ranging::CRangingClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Ranging Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeRanging].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_SpeedCtrl"){
    if(iMaCIClients[KTypeSpeedCtrl].find(aMaCISl.instance) == iMaCIClients[KTypeSpeedCtrl].end()){

      SpeedCtrl::CSpeedCtrlClient *c = new SpeedCtrl::CSpeedCtrlClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a SpeedCtrl Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeSpeedCtrl].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_Text"){
    if(iMaCIClients[KTypeText].find(aMaCISl.instance) == iMaCIClients[KTypeText].end()){

      Text::CTextClient *c = new Text::CTextClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Text Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeText].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_Unit"){
    if(iMaCIClients[KTypeUnit].find(aMaCISl.instance) == iMaCIClients[KTypeUnit].end()){

      Unit::CUnitClient *c = new Unit::CUnitClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Unit Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeUnit].insert(make_pair(aMaCISl.instance,c));
    }
  }else if(aMaCISl.interface == "MaCI_Wireless"){
    if(iMaCIClients[KTypeWireless].find(aMaCISl.instance) == iMaCIClients[KTypeWireless].end()){

      Wireless::CWirelessClient *c = new Wireless::CWirelessClient(iGIMI,aService.service.serviceminor);
      c->SetDefaultTarget(aService.gimnetid);
      dPrint(3,"'%s' not constructed, so Constructed a Wireless Client %p",aMaCISl.instance.c_str(),c);
      iMaCIClients[KTypeWireless].insert(make_pair(aMaCISl.instance,c));
    }
  }else{
    dPrintLCRed(1,"Unknown interface '%s'", aMaCISl.interface.c_str());
  }

}
//******************************************************************************

void CMachineCtrlClient::DestructAllClients(){
  
  
  
  TMaCIClientsMap::iterator iter = iMaCIClients.begin();
  
  while(iter != iMaCIClients.end()){
    for(EACH_IN_i(iter->second)){
      if(i->second  != NULL){
        
        delete(i->second);
      }
    }
    iter++;
  }
    


}
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
